from maix import image, display, app, time, camera ,uart ,touchscreen
from maix.v1.image import RGB2XYZ
import cv2
import numpy as np
import math
from math import *
from struct import pack
import re

ts = touchscreen.TouchScreen()
disp = display.Display()
cam = camera.Camera(320,240)
cam.luma(50)		    # 设置亮度，范围[0, 100]
cam.constrast(50)		# 设置对比度，范围[0, 100]
cam.saturation(50)		# 设置饱和度，范围[0, 100]


# 定义颜色阈值（HSV）
colorsholds = {
    'purple': {'lower': np.array([125, 43, 46]), 'upper': np.array([155, 255, 255])},
    'red1'  : {'lower': np.array([0  , 43, 46]), 'upper': np.array([10 , 255, 255])},
    'red2'  : {'lower': np.array([156, 43, 46]), 'upper': np.array([180, 255, 255])},
    'blue'  : {'lower': np.array([100, 43, 46]), 'upper': np.array([124, 255, 255])},
    'yellow': {'lower': np.array([26 , 43, 46]), 'upper': np.array([34 , 255, 255])},
    'black' : {'lower': np.array([0  ,  0,  0]), 'upper': np.array([180, 255,  46])}
}



def scan_color_plus(colorchoose):
    setcenter =[160,120]
    color = [[0,0],[0,0],[0,0]]
    min_area_threshold = 500  # 最小面积阈值
    max_area_threshold = 30000
    colors_to_detect = []  # 如果没有匹配的color_code，则不检测任何颜色
     # 初始化结果
    results = {'green': [0, 0], 'red': [0, 0], 'blue': [0, 0]}

    img =cam.read()
    img = img.lens_corr(strength=1.8)	# 调整strength的值直到画面不再畸变
    img = image.image2cv(img, ensure_bgr=True, copy=False)
    
    hsv_img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    if colorchoose ==0:
        colors_to_detect = ['red1','red2','purple']
        for color_name in colors_to_detect:
            if color_name == 'red1' or color_name == 'red2':
                if color_name == 'red1':
                    mask1 = cv2.inRange(hsv_img, colorsholds[color_name]['lower'], colorsholds[color_name]['upper'])
                elif color_name == 'red2':
                    mask2 = cv2.inRange(hsv_img, colorsholds[color_name]['lower'], colorsholds[color_name]['upper'])
                if 'mask1' in locals() and 'mask2' in locals():
                    mask = cv2.bitwise_or(mask1, mask2)
                    color_name = 'red'  # 统一处理为红色
                else:
                    continue
            else :
                mask = cv2.inRange(hsv_img, colorsholds[color_name]['lower'], colorsholds[color_name]['upper'])
            
            # 形态学操作：膨胀
            kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) 
            gaussianblur_mask = cv2.GaussianBlur(mask,(5,5),1)
            #morphologyEx_mask = cv2.morphologyEx(gaussianblur_mask,cv2.MORPH_CLOSE,kernel)
            erode_mask =cv2.dilate(gaussianblur_mask,kernel,iterations=3) 

            # 初始化最大面积和对应的轮廓
            max_area = 0
            largest_circle = None
                
            # 找到轮廓
            contours, _ = cv2.findContours(erode_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            for contour in contours:
                area = cv2.contourArea(contour)
                if color_name == 'purple':
                    if min_area_threshold <= area <= max_area_threshold*10 and area > max_area:
                        max_area = area
                        largest_circle = contour
                else :
                    if min_area_threshold <= area <= max_area_threshold and area > max_area:
                        max_area = area
                        largest_circle = contour
                        
            if largest_circle is not None:
                M = cv2.moments(largest_circle)
                if M["m00"] != 0:
                    center_x = int(M["m10"] / M["m00"])
                    center_y = int(M["m01"] / M["m00"])
                        
                    # 在原图上绘制轮廓
                    cv2.drawContours(img, [largest_circle], -1, (0, 255, 0), 2)
                    cv2.circle(img, (center_x, center_y), 5, (0, 0, 255), -1)
                    
                    if color_name == 'red':
                        color[0][0] = center_x -setcenter[0]
                        color[0][1] = center_y -setcenter[1]
                    if color_name == 'purple':
                        color[1][0] = center_x -setcenter[0]
                        color[1][1] = center_y -setcenter[1]
        
        img = image.cv2image(img)
        disp.show(img)
        print(color)
        return color        
    if colorchoose ==1:
        colors_to_detect = ['blue','purple']
        for color_name in colors_to_detect:
            mask = cv2.inRange(hsv_img, colorsholds[color_name]['lower'], colorsholds[color_name]['upper'])
            # 形态学操作：膨胀
            kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) #np.ones((5, 5), np.uint8)
            gaussianblur_mask = cv2.GaussianBlur(mask,(5,5),1)
            #morphologyEx_mask = cv2.morphologyEx(gaussianblur_mask,cv2.MORPH_CLOSE,kernel)
            erode_mask =cv2.dilate(gaussianblur_mask,kernel,iterations=3) 

            # 初始化最大面积和对应的轮廓
            max_area = 0
            largest_circle = None
            
            # 找到轮廓
            contours, _ = cv2.findContours(erode_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            for contour in contours:
                area = cv2.contourArea(contour)
                if color_name == 'purple':
                    if min_area_threshold *10 <= area <= max_area_threshold*10 and area > max_area:
                        max_area = area
                        largest_circle = contour
                else :
                    if min_area_threshold <= area <= max_area_threshold and area > max_area:
                        max_area = area
                        largest_circle = contour
                    
            if largest_circle is not None:
                M = cv2.moments(largest_circle)
                if M["m00"] != 0:
                    center_x = int(M["m10"] / M["m00"])
                    center_y = int(M["m01"] / M["m00"])
                    
                    # 在原图上绘制轮廓
                    cv2.drawContours(img, [largest_circle], -1, (0, 255, 0), 2)
                    cv2.circle(img, (center_x, center_y), 5, (0, 0, 255), -1)

                    if color_name == 'blue':
                        color[0][0] = center_x -setcenter[0]
                        color[0][1] = center_y -setcenter[1]
                    if color_name == 'purple': 
                        color[1][0] = center_x -setcenter[0]
                        color[1][1] = center_y -setcenter[1]

        img = image.cv2image(img)
        disp.show(img)
        print(color)
        return color



def is_in_button(x, y, btn_pos):
    return x > btn_pos[0] and x < btn_pos[0] + btn_pos[2] and y > btn_pos[1] and y < btn_pos[1] + btn_pos[3]

def colorselect():
    selectcolor =None
    while 1:
        img =cam.read()
        
        red_label = "RED"
        size = image.string_size(red_label)
        read_btn_pos = [0, 0, 30*2 + size.width(), 30 * 2 + size.height()]
        img.draw_string(5 , 5, red_label, image.COLOR_RED)
        img.draw_rect(read_btn_pos[0], read_btn_pos[1], read_btn_pos[2], read_btn_pos[3],  image.COLOR_RED, 2)

        blue_label = "BlUE"
        size = image.string_size(blue_label)
        blue_btn_pos = [0, 150, 30*2 + size.width(), 30 * 2 + size.height()]
        img.draw_string(5, 155, blue_label, image.COLOR_BLUE)
        img.draw_rect(blue_btn_pos[0], blue_btn_pos[1], blue_btn_pos[2], blue_btn_pos[3],  image.COLOR_BLUE, 2)

        x, y, pressed = ts.read()
        if is_in_button(x, y, read_btn_pos):
            selectcolor =0
        if is_in_button(x, y, blue_btn_pos):
            selectcolor =1

        if selectcolor != None:
            return selectcolor
            break
        disp.show(img)

def scan_safety(colorchoose):
    setcenter = [160, 120]
    color = [[0, 0], [0, 0], [0, 0]]
    min_area_threshold = 500  # 最小面积阈值

    img = cam.read()
    img = img.lens_corr(strength=1.8)  # 调整strength的值直到画面不再畸变
    img = image.image2cv(img, ensure_bgr=True, copy=False)

    hsv_img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

    # 先识别紫色区域
    purple_mask = cv2.inRange(hsv_img, colorsholds['purple']['lower'], colorsholds['purple']['upper'])
    kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
    purple_mask = cv2.dilate(purple_mask, kernel, iterations=3)

    # 找到紫色区域的轮廓
    contours, _ = cv2.findContours(purple_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    largest_purple_contour = None
    max_purple_area = 0

    for contour in contours:
        area = cv2.contourArea(contour)
        if area > max_purple_area:
            max_purple_area = area
            largest_purple_contour = contour

    if largest_purple_contour is not None:
        x, y, w, h = cv2.boundingRect(largest_purple_contour)
        purple_roi = hsv_img[y:y+h, x:x+w]

        # 根据colorchoose选择颜色
        if colorchoose == 0:
            colors_to_detect = ['red1', 'red2']
            mask1 = cv2.inRange(purple_roi, colorsholds['red1']['lower'], colorsholds['red1']['upper'])
            mask2 = cv2.inRange(purple_roi, colorsholds['red2']['lower'], colorsholds['red2']['upper'])
            mask = cv2.bitwise_or(mask1, mask2)
        elif colorchoose == 1:
            colors_to_detect = ['blue']
            mask = cv2.inRange(purple_roi, colorsholds['blue']['lower'], colorsholds['blue']['upper'])

        # 形态学操作：膨胀
        mask = cv2.dilate(mask, kernel, iterations=3)

        # 找到目标颜色的轮廓
        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        largest_target_contour = None
        max_target_area = 0

        for contour in contours:
            area = cv2.contourArea(contour)
            if area > max_target_area:
                max_target_area = area
                largest_target_contour = contour

        if largest_target_contour is not None:
            M = cv2.moments(largest_target_contour)
            if M["m00"] != 0:
                center_x = int(M["m10"] / M["m00"]) + x  # 加上紫色区域的x偏移
                center_y = int(M["m01"] / M["m00"]) + y  # 加上紫色区域的y偏移

                # 在原图上绘制轮廓和中心点
                cv2.drawContours(img, [largest_target_contour], -1, (0, 255, 0), 2)
                cv2.circle(img, (center_x, center_y), 5, (0, 0, 255), -1)

                color[0][0] = center_x - setcenter[0]
                color[0][1] = center_y - setcenter[1]

    img = image.cv2image(img)
    disp.show(img)
    print(color)
    return color
        
device = "/dev/serial0"
serial = uart.UART(device, 115200)

def recv_handle(data : bytes):
    RecvCode =None
    print(data)
    tmp = hex(data[0])
    if tmp =="0xb2" :
        RecvCode =0XB2   
        return RecvCode
    else :
        return RecvCode

#通信标准
#发送 
#0XAA 0X5A(数据码) (颜色(0XC0 red 0XC1 green 0XC2 blue) +对应颜色中心坐标) 0XBB

flag =0
colorchoose =None
safetyaeraflag=0

while not app.need_exit():
    data =serial.read(-1,1)
    if data != b'':
       RecvCode = recv_handle(data)
       if RecvCode ==0XB2 :
            safetyaeraflag=1

    if flag ==0:
        colorchoose = colorselect()
        if colorchoose !=None:
            flag =1
    
    if flag==1 and safetyaeraflag==0 :
        color =scan_color_plus(colorchoose)
        #color =scan_safety(colorchoose)
        bytes_content = b'\xAA\x5A'
        bytes_content +=b'\xC0'
        #red/blue
        bytes_content += pack("<h",int(color[0][0]))
        bytes_content += pack("<h",int(color[0][1]))
        #yellow
        bytes_content += b'\xC1'
        bytes_content += pack("<h",int(color[1][0]))
        bytes_content += pack("<h",int(color[1][1]))
        #black
        bytes_content += b'\xC2'
        bytes_content += pack("<h",int(color[2][0]))
        bytes_content += pack("<h",int(color[2][1]))
        bytes_content += b'\xBB'
        serial.write(bytes_content)
        #print(bytes_content)
    
    if flag ==1 and safetyaeraflag==1:
        color =scan_safety(colorchoose)
        bytes_content = b'\xAA\x5A'
        bytes_content +=b'\xB0'
        #red/blue
        bytes_content += pack("<h",int(color[0][0]))
        bytes_content += pack("<h",int(color[0][1]))
        bytes_content += b'\xBB'
        serial.write(bytes_content)
        print(bytes_content)
